/**
  ******************************************************************************
  * @author     Chris
  * @since      2024/6/24 10:21
  *
  * @file       hd_mpu6050.c
  * @brief      Xxx hardware driver.
  *
  * @note       This file contains the hardware driver for the Xxx.
  *
  * @warning    None.
  ******************************************************************************
  * Change Logs:
  *   Date          Author       Notes
  *   2024/6/24     Chris        the first version
  *
  ******************************************************************************
  */


#include "hd_mpu6050.h"

static void MPU6050_writeReg(MPU6050 *this, uint8_t regAddress, uint8_t dat) {
    this->i2c->send(this->i2c, MPU6050_ADDRESS, regAddress, &dat, 1);
}

static uint8_t MPU6050_readReg(MPU6050 *this, uint8_t regAddress) {
    uint8_t buf[2];
    this->i2c->read(this->i2c, MPU6050_ADDRESS, regAddress, buf, 1);
    return buf[0];
}

static void MPU6050_init(MPU6050 *this) {
    MPU6050_writeReg(this, MPU6050_PWR_MGMT_1, 0x01);
    MPU6050_writeReg(this, MPU6050_PWR_MGMT_2, 0x00);
    MPU6050_writeReg(this, MPU6050_SMPLRT_DIV, 0x09);
    MPU6050_writeReg(this, MPU6050_CONFIG, 0x06);
    MPU6050_writeReg(this, MPU6050_GYRO_CONFIG, 0x18);
    MPU6050_writeReg(this, MPU6050_ACCEL_CONFIG, 0x18);
}

static uint8_t MPU6050_getID(MPU6050 *this) {
    return MPU6050_readReg(this, MPU6050_WHO_AM_I);
}

static void MPU6050_getData(MPU6050 *this) {
    int16_t dataH;

    // Acceleration
    dataH = MPU6050_readReg(this, MPU6050_ACCEL_XOUT_H);
    this->accX = ( dataH << 8) | MPU6050_readReg(this, MPU6050_ACCEL_XOUT_L);

    dataH = MPU6050_readReg(this, MPU6050_ACCEL_YOUT_H);
    this->accY = (dataH << 8) | MPU6050_readReg(this, MPU6050_ACCEL_YOUT_L);

    dataH = MPU6050_readReg(this, MPU6050_ACCEL_ZOUT_H);
    this->accZ = (dataH << 8) | MPU6050_readReg(this, MPU6050_ACCEL_ZOUT_L);

    // Temperature
    dataH = MPU6050_readReg(this, MPU6050_TEMP_OUT_H);
    this->temp = (dataH << 8) | MPU6050_readReg(this, MPU6050_TEMP_OUT_L);

    // angular speed
    dataH = MPU6050_readReg(this, MPU6050_GYRO_XOUT_H);
    this->gyroX = (dataH << 8) | MPU6050_readReg(this, MPU6050_GYRO_XOUT_L);

    dataH = MPU6050_readReg(this, MPU6050_GYRO_YOUT_H);
    this->gyroY = (dataH << 8) | MPU6050_readReg(this, MPU6050_GYRO_YOUT_L);

    dataH = MPU6050_readReg(this, MPU6050_GYRO_ZOUT_H);
    this->gyroZ = (dataH << 8) | MPU6050_readReg(this, MPU6050_GYRO_ZOUT_L);
}

static MPU6050 *build(I2C *i2c) {
    MPU6050 *mpu6050 = malloc(sizeof(MPU6050));

    Driver_add(mpu6050, GEN_PERM);

    mpu6050->i2c      = i2c;
    mpu6050->init     = &MPU6050_init;
    mpu6050->get_id   = &MPU6050_getID;
    mpu6050->get_data = &MPU6050_getData;

    return mpu6050;
}

const struct MPU6050Class MPU6050Builder = {.build = &build};